package org.lovelandhs.robotics.y2010.drive;

import edu.wpi.first.wpilibj.Jaguar;
import org.lovelandhs.robotics.y2010.Robot;
import org.lovelandhs.robotics.y2010.TurnListener;
import org.lovelandhs.robotics.y2010.input.Axis;
import org.lovelandhs.robotics.y2010.input.Button;
import org.lovelandhs.robotics.y2010.input.ButtonListener;
import org.lovelandhs.robotics.y2010.input.InputManager;

/**
 *
 * @author Robotics
 */
public class Drive implements TurnListener, ButtonListener {

	private Jaguar frontLeft;
	private Jaguar frontRight;
	private Jaguar backLeft;
	private Jaguar backRight;

	private Direction direction;

	public Drive() {
		frontLeft = new Jaguar(1);
		frontRight = new Jaguar(2);
		backLeft = new Jaguar(6);
		backRight = new Jaguar(7);

		direction = Direction.FORWARD;

		Robot.get().addButtonListener("back", this);
	}

	public void drive(double hPower, double vPower, double turn) {
		frontLeft.set((vPower - hPower + turn) * direction.getModifier());
		frontRight.set((-vPower - hPower + turn) * direction.getModifier());
		backLeft.set((vPower + hPower + turn) * direction.getModifier());
		backRight.set((-vPower + hPower + turn) * direction.getModifier());
	}

	public void onAutonomousTurn(Robot robot) {

	}

	public void onTeleopTurn(Robot robot) {
		InputManager input = robot.getInput();

		Axis moveX = input.getAxis("LeftX");
		Axis moveY = input.getAxis("LeftY");
		Axis moveTheta = input.getAxis("Trigger");

		drive(moveX.getValue(), moveY.getValue(), moveTheta.getValue());
	}

	public Jaguar getFrontLeft() {
		return frontLeft;
	}

	public Jaguar getFrontRight() {
		return frontRight;
	}

	public Jaguar getBackLeft() {
		return backLeft;
	}

	public Jaguar getBackRight() {
		return backRight;
	}

	public Direction getDirection() {
		return direction;
	}

	public void setDirection(Direction direction) {
		this.direction = direction;
	}

	public void onButtonPressed(Button b) {
		if (direction == Direction.FORWARD) {
			setDirection(Direction.BACKWARD);
		} else if (direction == Direction.BACKWARD) {
			setDirection(Direction.FORWARD);
		}
	}

	public void onButtonReleased(Button b) {
	}

	public static class Direction {
		public static Direction FORWARD = new Direction(1);
		public static Direction BACKWARD = new Direction(-1);

		private int modifier;

		private Direction(int modifier) {
			this.modifier = modifier;
		}

		public int getModifier() {
			return modifier;
		}

	};

}
